/**
 * @(#) NXTCommand.java
 */

package pcsr.drivers.nxtDriver.command;

public class NXTCommand implements NXTProtocol, INXTCommand
{
	private INXTComm nxtComm;
	private boolean verifyCommand;
	private static NXTCommand singleton = new NXTCommand();

	@Override
	public int GetBatteryLevel( ) {
		byte [] request = {DIRECT_COMMAND_REPLY, GET_BATTERY_LEVEL};
		nxtComm.sendData(request);
		byte [] reply = nxtComm.readData();
		if(reply[1] != GET_BATTERY_LEVEL)
			System.out.println("Weird data reply received.");
		if(reply[2] != 0)
			System.out.println("NXT reports the check battery command did not work.");
		int batteryLevel = (0xFF & reply[3]) | ((0xFF & reply[4]) << 8);
		return batteryLevel;
	}

	@Override
	public DeviceInfo GetDeviceInfo( ) {
		// TODO !! Needs to check port to verify they are correct ranges.
		byte [] request = {SYSTEM_COMMAND_REPLY, GET_DEVICE_INFO};
		nxtComm.sendData(request);
		byte [] reply = nxtComm.readData();
		DeviceInfo d = new DeviceInfo();
		d.status = reply[2];
		d.SetNXTname(new StringBuffer(new String(reply)).delete(18,33).delete(0, 3).toString());
		d.bluetoothAddress = Integer.toHexString(reply[18]) + ":" + Integer.toHexString(reply[19]) + ":" + Integer.toHexString(reply[20]) + ":" + Integer.toHexString(reply[21]) + ":" + Integer.toHexString(reply[22]) + ":" + Integer.toHexString(reply[23]) + ":" + Integer.toHexString(reply[24]);
		d.SetSignalStrength((0xFF & reply[25]) | ((0xFF & reply[26]) << 8)| ((0xFF & reply[27]) << 16)| ((0xFF & reply[28]) << 24));
		d.freeFlash = (0xFF & reply[29]) | ((0xFF & reply[30]) << 8)| ((0xFF & reply[31]) << 16)| ((0xFF & reply[32]) << 24);
		return d;
	}

	@Override
	public InputValues GetInputValues( int port ) {
		// TODO		 !! Needs to check port to verify they are correct ranges.
		byte [] request = {DIRECT_COMMAND_REPLY, GET_INPUT_VALUES, (byte)port};
		nxtComm.sendData(request);
		byte [] reply = nxtComm.readData();
		InputValues inputValues = new InputValues();
		inputValues.inputPort = reply[3];
		// 0 is false, 1 is true.
		inputValues.valid = (reply[4] != 0);
		// 0 is false, 1 is true. 
		inputValues.isCalibrated = (reply[5] == 0);
		inputValues.sensorType = reply[6];
		inputValues.sensorMode = reply[7];
		inputValues.rawADValue = (0xFF & reply[8]) | ((0xFF & reply[9]) << 8);
		inputValues.normalizedADValue = (0xFF & reply[10]) | ((0xFF & reply[11]) << 8);
		inputValues.scaledValue = (short)((0xFF & reply[12]) | (reply[13] << 8));
		inputValues.calibratedValue = (short)((0xFF & reply[14]) | (reply[15] << 8));
		// * Untested if scaledValue and calibrateValue above work as shown. Alt code below.
		//inputValues.scaledValue = (short)((0xFF & reply[12]) | ((0xFF & reply[13]) << 8));
		//inputValues.calibratedValue = (short)((0xFF & reply[14]) | ((0xFF & reply[15]) << 8));
		
		return inputValues;
	}

	@Override
	public OutputState GetOutputState( int port ) {
		// TODO !! Needs to check port to verify they are correct ranges.
		byte [] request = {DIRECT_COMMAND_REPLY, GET_OUTPUT_STATE, (byte)port};
		nxtComm.sendData(request);
		byte [] reply = nxtComm.readData();
		
		if(reply[1] != GET_OUTPUT_STATE) {
			System.out.println("Oops! Error in NXTCommand.getOutputState.");
			System.out.println("Return data did not match request.");
			System.out.println("reply[0] = " + reply[0] + "  reply[1] = " + reply[1] +"  reply[2] = " + reply[2]);
		}
		OutputState outputState = new OutputState(port);
		outputState.status = reply[2];
		outputState.outputPort = reply[3];
		outputState.powerSetpoint = reply[4];
		outputState.mode = reply[5];
		outputState.regulationMode = reply[6];
		outputState.turnRatio = reply[7];
		outputState.runState = reply[8];
		outputState.tachoLimit = (0xFF & reply[9]) | ((0xFF & reply[10]) << 8)| ((0xFF & reply[11]) << 16)| ((0xFF & reply[12]) << 24);
		outputState.tachoCount = (0xFF & reply[13]) | ((0xFF & reply[14]) << 8)| ((0xFF & reply[15]) << 16)| ((0xFF & reply[16]) << 24);
		outputState.blockTachoCount = (0xFF & reply[17]) | ((0xFF & reply[18]) << 8)| ((0xFF & reply[19]) << 16)| ((0xFF & reply[20]) << 24);
		outputState.rotationCount = (0xFF & reply[21]) | ((0xFF & reply[22]) << 8)| ((0xFF & reply[23]) << 16)| ((0xFF & reply[24]) << 24);
		return outputState;
	}

	@Override
	public boolean IsVerify( ) {
		return verifyCommand;
	}

	@Override
	public byte[] LSGetStatus( byte port ) {
		byte [] request = {DIRECT_COMMAND_REPLY, LS_GET_STATUS, port};
		nxtComm.sendData(request);
		byte [] reply = nxtComm.readData();
		if (reply[2] == ErrorMessages.COMMUNICATION_BUS_ERROR)
			System.out.println("NXTCommand.LSGetStatus() error: Communications Bus Error");
		else if(reply[2] == ErrorMessages.PENDING_COMMUNICATION_TRANSACTION_IN_PROGRESS)
			System.out.println("NXTCommand.LSGetStatus() error: Pending communication transaction in progress");
		else if(reply[2] == ErrorMessages.SPECIFIED_CHANNEL_CONNECTION_NOT_CONFIGURED_OR_BUSY)
			System.out.println("NXTCommand.LSGetStatus() error: Specified channel connection not configured or busy");
		else if(reply[2] != 0)
			System.out.println("NXTCommand.LSGetStatus() Error Number " + reply[2]);
		byte [] returnData = {reply[2], reply[3]}; 
		return returnData;
	}

	@Override
	public byte[] LSRead( byte port ) {
		byte [] request = {DIRECT_COMMAND_REPLY, LS_READ, port};
		nxtComm.sendData(request);
		byte [] reply = nxtComm.readData();
		
		byte rxLength = reply[3];
		byte [] rxData = new byte[rxLength];
		if(reply[2] == 0) {
			System.arraycopy(reply, 4, rxData, 0, rxLength);
		} else if(reply[2] == ErrorMessages.SPECIFIED_CHANNEL_CONNECTION_NOT_CONFIGURED_OR_BUSY) {
			System.out.println("NXTCommand.LSRead error: Specified channel connection not configured or busy.");
			return null;
		} else {
			System.out.println("NXTCommand.LSRead error: " + reply[2]);
			return null;
		}
		return rxData;
	}

	@Override
	public byte LSWrite( byte port, byte[] txData, byte rxDataLength ) {
		byte [] request = {DIRECT_COMMAND_NOREPLY, LS_WRITE, port, (byte)txData.length, rxDataLength};
		request = appendBytes(request, txData);
		return sendRequest(request);
	}

	@Override
	public byte PlayTone( int frequency, int duration ) {
		byte [] request = {DIRECT_COMMAND_NOREPLY, PLAY_TONE, (byte)frequency, (byte)(frequency>>>8), (byte)duration, (byte)(duration>>>8)};
		return sendRequest(request);
	}

	@Override
	public byte ResetMotorPosition( int port, boolean relative ) {
		// TODO !! Needs to check port to verify they are correct ranges.
		// !!! I'm not sure I'm sending boolean properly
		byte boolVal = 0;
		if(relative) boolVal = (byte)0xFF;
		byte [] request = {DIRECT_COMMAND_NOREPLY, RESET_MOTOR_POSITION, (byte)port, boolVal};
		return sendRequest(request);
	}

	@Override
	public byte SetInputMode( int port, int sensorType, int sensorMode ) {
		// TODO !! Needs to check port to verify they are correct ranges.
		byte [] request = {DIRECT_COMMAND_NOREPLY, SET_INPUT_MODE, (byte)port, (byte)sensorType, (byte)sensorMode};
		return sendRequest(request);
	}

	@Override
	public byte SetOutputState( int port, byte power, int mode, int regulationMode, int turnRatio, int runState, int tachoLimit ) {
		// TODO !! Needs to check port, power to verify they are correct ranges.
		byte [] request = {DIRECT_COMMAND_NOREPLY, SET_OUTPUT_STATE, (byte)port, power, (byte)mode, (byte)regulationMode, (byte)turnRatio, (byte)runState, (byte)tachoLimit, (byte)(tachoLimit>>>8), (byte)(tachoLimit>>>16), (byte)(tachoLimit>>>24)};
		return sendRequest(request);
	}

	private byte[] appendBytes( byte[] request, byte[] txData ) {
		byte [] array = new byte[request.length + txData.length];
		System.arraycopy(request, 0, array, 0, request.length);
		System.arraycopy(txData, 0, array, request.length, txData.length);
		return array;
	}

	/**
	 * Small helper method to send request to NXT and return verification result.
	 * @param request
	 * @return
	 */
	private byte sendRequest( byte[] request ) {
		byte verify = 0; // default of 0 means success
		if(verifyCommand)
			request[0] = DIRECT_COMMAND_REPLY;
		
		nxtComm.sendData(request);
		if(verifyCommand) {
			byte [] reply = nxtComm.readData();
			verify = reply[2];
		}
		return verify;
	}


	@Override
	public void SetVerify( boolean verify ) {
		verifyCommand = verify;
	}
		
	/**
	 * Opens a connection using iCommand.
	 * As of Sept-16-2007 this method throws an
	 * exception rather than System.exit() so that user interfaces
	 * can handle errors.
	 * @throws Exception When open fails.
	 */
	@Override
	public void Open( ) throws RuntimeException{

		singleton.nxtComm = NXTCommFactory.createInstance();
		
		try {
			singleton.nxtComm.open();
		} catch (Exception e) {
		 	//System.err.println("NXTCommand.open(): Error while connecting ...");
			//e.printStackTrace();
			//System.exit(-1);
			throw new RuntimeException(e);
		}
	}

	/**
	 * Call the close() command when your program ends, otherwise you
	 * will have to turn the NXT brick off/on before you run another
	 * program using iCommand.
	 * 
	 */
	@Override
	public void Close( ) {
		
		// Set all motors to float mode:
		byte ALL_MOTORS = (byte)0xFF;
		singleton.SetOutputState(ALL_MOTORS, (byte)0, 0x00, REGULATION_MODE_IDLE, 0, MOTOR_RUN_STATE_IDLE, 0);
		
		// Signal to iCommand thru LCP to close connection first:
		// 1. Check if brick has NXJ or standard firmware.
		//FirmwareInfo fw = NXTCommand.getSingleton().getFirmwareVersion();
		//System.out.println("FW_VERSION: " + fw.firmwareVersion);
		//System.out.println("PROTOCOL_VERS: " + fw.protocolVersion);
		
		// 2. If NXJ, then send LCP command NXJ_DISCONNECT = 0x20
		
		singleton.nxtComm.close();
	}

	public static INXTCommand GetSingleton( ) {
		return singleton;
	}
}
